#include "motorcontroller.h"
#include <QSerialPort>
#include <QDebug>
#include <QMap>
#include <QCoreApplication>
#include <QTimer>
#include <QThread>

MotorController::MotorController(QObject *parent) 
    : QObject(parent), 
      serialPort(new QSerialPort(this)),
      portName("/dev/ttyS9"),
      baudRate(115200),
      defaultAcceleration(150) {}

MotorController::MotorController(const QString &port, int baudrate, 
                               int acceleration, QObject *parent)
    : QObject(parent),
      serialPort(new QSerialPort(this)),
      portName(port),
      baudRate(baudrate),
      defaultAcceleration(acceleration) {}

MotorController::~MotorController() {
    close();
}

bool MotorController::open() {
    if (serialPort->isOpen()) return true;
    serialPort->setPortName(portName);
    serialPort->setBaudRate(baudRate);
    if (!serialPort->open(QIODevice::ReadWrite)) {
        qWarning() << "打开串口失败:" << serialPort->errorString();
        return false;
    }
    return true;
}

void MotorController::close() {
    if (serialPort->isOpen()) {
        serialPort->close();
    }
}

bool MotorController::sendData(const QByteArray &data) {
    Q_ASSERT(thread() == QThread::currentThread());
    if (!open()) return false;

    if (serialPort->write(data) != data.size()) {
        qWarning() << "串口发送失败:" << serialPort->errorString();
        return false;
    }
    return serialPort->waitForBytesWritten(100);
}

void MotorController::syncEnable() {
    QByteArray cmd;
    cmd.append(char(0x00));
    cmd.append(char(0xFF));
    cmd.append(char(0x66));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::setVelocity(int addr, int direction, int velocity, int acceleration) {
    int acc = (acceleration == -1) ? defaultAcceleration : acceleration;
    QByteArray cmd;
    cmd.append(char(addr));
    cmd.append(char(0xF6));
    cmd.append(char(direction));
    cmd.append(char((velocity >> 8) & 0xFF));
    cmd.append(char(velocity & 0xFF));
    cmd.append(char(acc));
    cmd.append(char(0x01));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::setPosition(int addr, int direction, int velocity, 
                                int position, bool absolute, int acceleration) {
    int actualDir = direction;
    int actualPos = position;
    if (!absolute && position < 0) {
        actualDir = 1 - direction;
        actualPos = -position;
    } else if (absolute && position < 0) {
        qWarning() << "绝对位置不能为负数";
        return;
    }
    
    int acc = (acceleration == -1) ? defaultAcceleration : acceleration;
    int absFlag = absolute ? 1 : 0;
    
    QByteArray cmd;
    cmd.append(char(addr));
    cmd.append(char(0xFD));
    cmd.append(char(actualDir));
    cmd.append(char((velocity >> 8) & 0xFF));
    cmd.append(char(velocity & 0xFF));
    cmd.append(char(acc));
    cmd.append(char((actualPos >> 24) & 0xFF));
    cmd.append(char((actualPos >> 16) & 0xFF));
    cmd.append(char((actualPos >> 8) & 0xFF));
    cmd.append(char(actualPos & 0xFF));
    cmd.append(char(absFlag));
    cmd.append(char(0x01));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::stop(int addr) {
    QByteArray cmd;
    cmd.append(char(addr));
    cmd.append(char(0xFE));
    cmd.append(char(0x98));
    cmd.append(char(0x01));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::originTriggerReturn(int addr, int o_mode, bool snF) {
    QByteArray cmd;
    cmd.append(char(addr));
    cmd.append(char(0x9A));
    cmd.append(char(o_mode));
    cmd.append(char(snF ? 0x01 : 0x00));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::originSetZero(int addr, bool svF) {
    QByteArray cmd;
    cmd.append(char(addr));
    cmd.append(char(0x93));
    cmd.append(char(0x88));
    cmd.append(char(svF ? 0x01 : 0x00));
    cmd.append(char(0x6B));
    sendData(cmd);
}

void MotorController::syncVelocity(const QVector<MotorSpeedParam> &motors, int velocity) {
    for (const auto &motor : motors) {
        setVelocity(motor.addr, motor.direction, velocity);
        QCoreApplication::processEvents();
        QThread::msleep(10);
    }
    syncEnable();
    QCoreApplication::processEvents();
    QThread::msleep(10);
}

void MotorController::syncPosition(const QVector<MotorPositionParam> &motors, 
                                 int velocity, bool absolute) {
    for (const auto &motor : motors) {
        setPosition(motor.addr, motor.direction, velocity, 
                   motor.position, absolute);
        QCoreApplication::processEvents();
        QThread::msleep(10);
    }
    syncEnable();
    QCoreApplication::processEvents();
    QThread::msleep(10);
}

void MotorController::moveDown() {
    setPosition(1, 0, 150, 3200, true);
}

void MotorController::moveUp() {
    setPosition(1, 0, 150, 0, true);
}

void MotorController::moveX(int position, int velocity, int motor1_addr, 
                          int motor1_dir, int motor2_addr, int motor2_dir, bool absolute) {
    QVector<MotorPositionParam> motors;
    motors.append({motor1_addr, motor1_dir, position});
    motors.append({motor2_addr, motor2_dir, position});
    syncPosition(motors, velocity, absolute);
}

void MotorController::moveXY(int x_position, int y_position, int velocity, bool absolute) {
    x_position = qBound(0, x_position, 30000);
    y_position = qBound(0, y_position, 30000);

    QVector<MotorPositionParam> motors;
    motors.append({3, 0, x_position});
    motors.append({4, 1, x_position});
    motors.append({2, 0, y_position});
    syncPosition(motors, velocity, absolute);
}
void MotorController::moveZ(int z_position, int velocity, bool absolute) {
    z_position = qBound(0, z_position, 30000);

    QVector<MotorPositionParam> motors;
    motors.append({1, 0, z_position});
    syncPosition(motors, velocity, absolute);
}

void MotorController::moveAxis(const QString &axis_name, int position, 
                             int velocity, bool absolute) {
    QMap<QString, QVector<MotorPositionParam>> axisConfig;
    axisConfig["x"] = QVector<MotorPositionParam>{{3, 0, position}, {4, 1, position}};
    axisConfig["y"] = QVector<MotorPositionParam>{{2, 0, position}};
    axisConfig["z"] = QVector<MotorPositionParam>{{1, 0, position}};

    if (!axisConfig.contains(axis_name)) {
        qWarning() << "无效的轴名称:" << axis_name;
        return;
    }
    syncPosition(axisConfig[axis_name], velocity, absolute);
}

void MotorController::moveAxis(const QStringList &axis_names, 
                             const QMap<QString, int> &positions, 
                             int velocity, bool absolute) {
    QVector<MotorPositionParam> motors;
    for (const QString &axis : axis_names) {
        if (!positions.contains(axis)) {
            qWarning() << "缺失轴" << axis << "的位置值";
            return;
        }
        int pos = positions[axis];
        if (axis == "x") {
            motors.append({3, 0, pos});
            motors.append({4, 1, pos});
        } else if (axis == "y") {
            motors.append({2, 0, pos});
        } else if (axis == "z") {
            motors.append({1, 0, pos});
        } else {
            qWarning() << "无效的轴名称:" << axis;
            return;
        }
    }
    syncPosition(motors, velocity, absolute);
}
